#include "servo.h"
unsigned int Servo_Angle = 0;//舵机角度
/******************************************************************
   配置占空比 范围 0 ~ (per-1)
   t = 0.5ms-------------------舵机会转动 0 °
   t = 1.0ms-------------------舵机会转动 45°
   t = 1.5ms-------------------舵机会转动 90°
   t = 2.0ms-------------------舵机会转动 135°
   t = 2.5ms-------------------舵机会转动180°
******************************************************************/

/******************************************************************
 * 函 数 名 称：Set_Servo_Angle
 * 函 数 说 明：设置角度
 * 函 数 形 参：angle=要设置的角度，范围40-140
 * 函 数 返 回：无
******************************************************************/
void Servo_Set_Angle(uint32_t angle)
{
    uint32_t period = 400;

    if(angle > 140)
    {
        angle = 140; // 限制角度在40到140度之间
    }
    if(angle < 40)
    {
        angle = 40; // 限制角度在40到140度之间
    }

    Servo_Angle = angle;

    // 计算PWM占空比
    // 0.5ms对应的计数 = 10
    // 2.5ms对应的计数 = 50
    float min_count = 10.0f;
    float max_count = 50.0f;
    float range = max_count - min_count;
    float ServoAngle = min_count + (((float)angle / 180.0f) * range);

    DL_TimerG_setCaptureCompareValue(PWM_SG90_INST, (uint32_t)(ServoAngle + 0.5f), GPIO_PWM_SG90_C0_IDX);
}

/******************************************************************
 * 函 数 名 称：读取当前角度
 * 函 数 说 明：Get_Servo_Angle
 * 函 数 形 参：无
 * 函 数 返 回：当前角度
 * 备       注：使用前必须确保之前使用过
                void Set_Servo_Angle(unsigned int angle)
                函数设置过角度
******************************************************************/
unsigned int Servo_Get_Angle(void)
{
    return Servo_Angle;
}